/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
# example client commanding joint group with swig wrapped c++ library in python
import sys, time
sys.path.append("../../../interfaces/wrap/Python/lib")
sys.path.append("../../../../GIMI/wrap/Python/lib")
import JointGroupCtrlClient
import gimi
#if sys.argv.__len__() != 3:
#	print "Usage: python %s clientName serverName"%sys.argv[0]
#	sys.exit()
g = gimi.GIMI()
jgcc = JointGroupCtrlClient.CJointGroupCtrlClient(g, 0)
print "connecting... ",
#ret = g.connectToHubEx("asrobo.hut.fi", 40002, sys.argv[1])
ret = g.connectToHubEx("asrobo.hut.fi", 40002, "")
print JointGroupCtrlClient.GetErrorStr(ret)
print "set target"
#jgcc.SetDefaultTarget(sys.argv[2])
jgcc.SetDefaultTarget("joidum")
print "open"
ret = jgcc.Open()
print JointGroupCtrlClient.GetErrorStr(ret)
del ret

print "getting position... ",
j = 9.9
ret = jgcc.GetPosition(0,j,1000)
print "success?",
print ret[0]
print "postion of joint 0:%f"%ret[1]

#set pos
j = 0.3
print "setting position to %f... "%j,
ret = jgcc.SetPosition(j,0)
print "success?",
print ret


#get speed
#GetSpeed (const int aJointNumber, float &aSpeed, unsigned int aTimeout_ms)
print "getting speed... ",
j = 9.9
ret = jgcc.GetSpeed(0,j,1000)
print "success?",
print ret[0]
print "speed of joint 0:%f"%ret[1]


#set speed
j = 0.0
print "setting speed to %f... "%j,
ret = jgcc.SetSpeed(j,0)
print "success?",
print ret

#get info
#GetJointGroupInformation(MaCI::JointGroupCtrl::TJointGroupInformation &INOUT, unsigned int aTimeout_ms);
#print "get",
#i = JointGroupCtrlClient.TJointGroupInformation()
#print "joint group info",
#ret = jgcc.GetJointGroupInformation(i, 1000)
#print ret
#print i

print "get info of ",
i = jgcc.GetJointNumber()
print "%d joints:"%i
print "*********************************"
info = JointGroupCtrlClient.TJointInfo("huhuu",5, JointGroupCtrlClient.KJointControlAngular)
for n in range(i):
	ret = jgcc.GetJointInformation(info, n, 1000)
	print "succes for joint", n," : ", ret,
	print " (%d)"%info.jointNumber
	print "min %f and max %f of %s"%(info.minPosition, info.maxPosition, info.jointName)
	print "can control speed: ", info.canControlSpeed
	print "can control position", info.canControlPosition
	print "max speed :", info.maxSpeed
	if info.jointControlType == JointGroupCtrlClient.KJointControlAngular:
		print "this is angular joint"
	elif info.jointControlType == JointGroupCtrlClient.KJointControlLinear:
		print "this is Linear joint"
	else: print "unknown joint type"
	
	print "*********************************"




print "Closing "
jgcc.Close()
print "Closed "
